/**
  ******************************************************************************
  * @file           : auto_patrol_task.cpp
  * @author         : Tessia
  * @brief          : None
  * @attention      : None
  * @date           : 2024/3/9
  ******************************************************************************
  */



#include "auto_patrol_task.h"
extern uint8_t VisionControlFlag;
extern rc_device_t g_rc;
extern float yaw_set_gyro;
extern float pitch_set_gyro;

extern float PITCH_GYRO_MAX;
extern float PITCH_GYRO_MIN;

extern float YAW_GYRO_MAX;
extern float YAW_GYRO_MIN;
extern float YAW_GYRO_MINI_CAMERA_MAX;
extern float YAW_GYRO_MINI_CAMERA_MIN;

extern judge_info_0 judgeInfo0;
extern judge_info_1 judgeInfo1;

extern twins_receive_info twins_receive;

extern uint8_t is_temporary_pause;

extern uint8_t mini_camera_0_enemy_data[12];
extern uint8_t armor_number_no_zero;

int pitch_increment = 1;//pitch自增标志位
int yaw_increment = 1;//yaw自增标志位

void variate_between_max_min(float &variate, float max, float min, float value, int &variate_increment){
    variate_increment = variate > max ? -1 : (variate < min ? 1 : variate_increment);
    variate = variate > max ? max : (variate < min ? min : variate);
    variate += value * variate_increment;
}

float compare_and_return_smaller(float a, float b){
    if(a > b)
        return b;
    else
        return a;
}

float compare_closest(float A,float B,float current){
    if(abs(A - current) < abs(B - current)){
        return(A);
    }else{
        return(B);
    }
}


float smaller_solution_cal(float target, float current){ //实则不是最优解，左头在右头发现目标后角度始终应该减小
    float round_target1 = floor(current) + target;
    float round_target2 = floor(current) + target - 1;
    float round_target3 = floor(current) + target + 1;

    float rounds_target = compare_and_return_smaller(compare_and_return_smaller(round_target1, round_target2), round_target3);
    return(rounds_target);
}

float rounds2currentRounds(float rounds){  //返回值为0-1
    float currentRounds;
    currentRounds = rounds - floor(rounds);
    return(currentRounds);
}

void rounds_optimal_solute(float total_rounds, float target_current_rounds, float &target_total_rounds){
    float target_cal = rounds2currentRounds(target_current_rounds);

    float round_target1 = floor(total_rounds) + target_cal;
    float round_target2 = floor(total_rounds) + target_cal - 1;
    float round_target3 = floor(total_rounds) + target_cal + 1;
    target_total_rounds = compare_closest(compare_closest(round_target1, round_target2, total_rounds), round_target3, total_rounds);
}

float left_optimal_solute(uint8_t is_another_left_of_axis, float this_target_current_round, float this_current_total_rounds){ //左头最优转向解算
    float this_target_total_rounds;
    if(is_another_left_of_axis == 1){ //此时右头目标在中轴线偏左，应找最近解
        rounds_optimal_solute(this_current_total_rounds, this_target_current_round, this_target_total_rounds);
    }else{ //此时右头目标在中轴线偏右，应该找稍小解
        this_target_total_rounds = smaller_solution_cal(this_target_current_round, this_current_total_rounds);
    }
    return (this_target_total_rounds);
}


void auto_patrol_task(void *argument){
    for(;;){
        if(g_rc.is_lost == 0 && g_rc.rc_info.sw.sw_r == RC_SW_UP && g_rc.rc_info.sw.sw_l == RC_SW_MID){ //当右摇杆拨到最上，左摇杆拨到最下时启动自动巡逻
            if((VisionControlFlag == 0 && is_temporary_pause == 0 && mini_camera_0_enemy_data[0] == 0 && judgeInfo0.current_HP != 0 && twins_receive.go_attack_outpost == 0) || AimAutoData.AutoAim == 0){ //目标未出现且不在视觉暂留状态，持续巡逻
                variate_between_max_min(pitch_set_gyro, PITCH_GYRO_MAX / 180.f - PATROL_PITCH_MAX_OFFSET, PITCH_GYRO_MIN / 180.f + PATROL_PITCH_MIN_OFFSET,
                                        1 * 6e-4, pitch_increment);
                variate_between_max_min(yaw_set_gyro, YAW_GYRO_MAX / 360.f - PATROL_YAW_OFFSET, YAW_GYRO_MIN / 360.f + PATROL_YAW_OFFSET,
                                        1 * 4e-4, yaw_increment);
            }else if(VisionControlFlag == 0 && is_temporary_pause == 0 && mini_camera_0_enemy_data[0] == 0 && judgeInfo0.current_HP != 0 && twins_receive.go_attack_outpost != 0){
                variate_between_max_min(pitch_set_gyro, PITCH_GYRO_MAX / 180.f - OUTPOST_PITCH_MAX_OFFSET,
                                        PITCH_GYRO_MIN / 180.f + OUTPOST_PITCH_MIN_OFFSET,
                                        6e-4, pitch_increment);
                variate_between_max_min(yaw_set_gyro, YAW_GYRO_MAX / 360.f - PATROL_YAW_OFFSET, YAW_GYRO_MIN / 360.f + PATROL_YAW_OFFSET,
                                        4e-4, yaw_increment);
            }
        }

        if(g_rc.is_lost == 0 && VisionControlFlag == 0 && twins_receive.is_target_detected == 1 && is_temporary_pause == 0){
            pitch_set_gyro = twins_receive.twins_pitch / 180.f;
            yaw_set_gyro = left_optimal_solute(twins_receive.is_left_of_axis, twins_receive.twins_yaw / 360.f, INS.My_Yaw / 360.f);
        }else if(g_rc.is_lost == 0 && VisionControlFlag == 0 && twins_receive.is_target_detected == 0 && is_temporary_pause == 0 && mini_camera_0_enemy_data[0] == 1 && AimAutoData.AutoAim == 1 && twins_receive.go_attack_outpost == 0){
            variate_between_max_min(yaw_set_gyro, YAW_GYRO_MINI_CAMERA_MAX / 360.f, YAW_GYRO_MINI_CAMERA_MIN / 360.f,
                                    1 * 4e-4, yaw_increment);
            variate_between_max_min(pitch_set_gyro, PITCH_GYRO_MAX / 180.f - PATROL_PITCH_MAX_OFFSET, 0.f / 180.f,
                                    1 * 8e-4, pitch_increment);
        }

        osDelay(1);
    }
}